#ifndef GRTRAVERSALBFS_H_
#define GRTRAVERSALBFS_H_

#include <vector>
#include <assert.h>

#include "graph_support/GraphSupport.h"

#include "graph_support/alg/traversal/AlwaysTrueFilter.h"

namespace GRAPH_SUPPORT_NS
{
namespace alg
{
namespace traversal
{

template< typename GraphT, typename ForEachFnT, typename FilterT >
void GrTraversalBFS( GraphT & gr, typename GraphT::GrVertex startVert,
		ForEachFnT forEachFn,
		FilterT edgesFilter )
{
	typedef GraphT Graph;
	typedef typename Graph::GrVertex GrVertex;
	typedef typename Graph::GrEdge GrEdge;
	typedef typename Graph::GrEdgePtr GrEdgePtr;
	typedef typename Graph::EdgeIterator EdgeIterator;

	int verticesCount = gr.getVertexCount();
	char BFS_NEW = 0;
	char BFS_QUEUED = 1;
	char BFS_COMPLETED = 2;
	std::vector< char > bfsStateBuf( verticesCount, BFS_NEW );
	char * bfsState = & bfsStateBuf[0];

	std::vector< size_t > queueBuf( verticesCount );
	size_t * qibeg = & queueBuf[0];
	size_t * qlim = qibeg + verticesCount;
	size_t * qhead = qibeg;
	size_t * qtail = qibeg;

	size_t startVid = gr.vert2num( startVert );
	* qtail = startVid;
	qtail++;
	bfsState[startVid] = BFS_QUEUED;

	while ( qtail > qhead )
	{
		size_t cur = * qhead;
		assert( bfsState[cur] == BFS_QUEUED );
		qhead++;

		EdgeIterator stopIter = gr.getEdgsEndId( cur );
		for ( EdgeIterator it = gr.getEdgsBegId( cur ); it != stopIter; ++it )
		{
			GrEdgePtr edgePtr = * it;
			if ( ! edgesFilter( edgePtr ) )
			{
				continue;
			}

			size_t endVid = edgePtr->endId();
			if ( ! bfsState[endVid] )
			{
				forEachFn( edgePtr );
				assert( qtail < qlim );
				* qtail = endVid;
				qtail++;
				bfsState[endVid] = BFS_QUEUED;
			}
		}

		bfsState[cur] = BFS_COMPLETED;
	}
}

template< typename GraphT, typename ForEachFnT >
void GrTraversalBFSAllEdges(
		GraphT & gr, typename GraphT::GrVertex startVert,
		ForEachFnT forEachFn )
{
	GrTraversalBFS( gr, startVert, forEachFn, & alwaysTrueFilter< typename GraphT::GrEdgePtr > );
}

}
}
}

#endif /* GRTRAVERSALBFS_H_ */
